#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
from geometry_msgs.msg import Twist,PoseStamped
from std_msgs.msg import Bool, Float32  
import airsim
from rclpy.qos import QoSProfile



class AirSimROS2Control(Node):
    def __init__(self):
        super().__init__('airsim_ros2_control')
        self.client = airsim.MultirotorClient()
        self.client.confirmConnection()
        self.client.enableApiControl(True)
        self.client.armDisarm(True)
        self.create_subscription(Twist, '/airsim/cmd_vel', self.control_callback, 10)
        self.create_subscription(Float32, '/airsim/takeoff', self.takeoff_callback, 10) 
        self.create_subscription(Bool, '/airsim/land', self.land_callback, 10) 
        # self.create_subscription(PoseStamped, '/airsim/target_point', self.move_to_pose_callback, 10) 
        self.create_subscription(PoseStamped,"/airsim/pose",self.pose_callback,10)
        # self.goal_reached_pub = self.create_publisher(Bool, '/airsim/goal_reached', QoSProfile(depth=10))
        self.get_logger().info('AirSim ROS2 Control Node Initialized.')
        self.takeoff_flag = False
        self.land_flag = False
        self.current_pose = (0.0,0.0,0.0)
        self.target_pose = (0.0,0.0,0.0)
    
    def pose_callback(self, msg):
        x = msg.pose.position.x
        y = msg.pose.position.y
        z = msg.pose.position.z
        self.current_pose = (x,y,z)
        
    def control_callback(self, msg):
        vx = msg.linear.x
        vy = msg.linear.y
        vz = -msg.linear.z
        yaw_rate = msg.angular.z

        self.client.moveByVelocityAsync(vx, vy, vz, 0.1, drivetrain=airsim.DrivetrainType.MaxDegreeOfFreedom,yaw_mode=airsim.YawMode(is_rate=False, yaw_or_rate=yaw_rate))#is_rate=False则yaw_or_rate=yaw_rate表示角度
        self.get_logger().info(f'Moving with vx: {vx}, vy: {vy}, vz: {vz}, yaw_rate: {yaw_rate}')
    
    def calculate_distance(self,pos1, pos2):
        """计算两点之间的欧几里得距离"""
        return ((pos1[0] - pos2[0]) ** 2 +
                (pos1[1] - pos2[1]) ** 2 +
                (pos1[2] - pos2[2]) ** 2) ** 0.5
    
    def move_to_pose_callback(self, msg):
        x = msg.pose.position.x
        y = msg.pose.position.y
        z = msg.pose.position.z
        self.target_pose = (x,y,z)
        self.get_logger().info(f'Moving to position x: {x}, y: {y}, z: {z}.')
        self.client.moveToPositionAsync(x, y, z, 2.0)
        # 检查是否接近目标点
        distance = self.calculate_distance(self.current_pose, self.target_pose)
        if distance < 1.0:
            for i in range(10):
                self.goal_reached_pub.publish(Bool(data=True))
                
    def takeoff_callback(self, msg):
        if msg.data and not self.takeoff_flag:
            self.takeoff_flag = True
            self.land_flag = False
            altitude = msg.data
            self.takeoff(altitude)#此处直接复用为上升的高度，避免自建msg
            
    def land_callback(self, msg):
        if msg.data and not self.land_flag:
            self.land_flag = True
            self.takeoff_flag = False
            self.land()

    def takeoff(self, altitude=30.0, speed=2.0):
        self.client.takeoffAsync().join()
        self.client.moveToZAsync(-altitude, speed).join()
        self.get_logger().info(f'Takeoff to altitude: {altitude} meters at speed: {speed} m/s.')

    def land(self):
        self.client.landAsync().join()
        self.client.armDisarm(False)
        self.client.enableApiControl(False)
        self.get_logger().info('Landing completed.')

def main(args=None):
    rclpy.init(args=args)
    airsim_control_node = AirSimROS2Control()
    airsim_control_node.takeoff(30.0)
    try:
        rclpy.spin(airsim_control_node)
    except KeyboardInterrupt:
        airsim_control_node.get_logger().info('Shutting down AirSim ROS2 Control Node.')
    finally:
        airsim_control_node.destroy_node()
        rclpy.shutdown()

if __name__ == '__main__':
    main()
